#include "TriGrid.h"
#include "Ray.h"
#include <limits>
using namespace geom;
using namespace util;
TriGrid::TriGrid(const MinMax2DBox &box, size_t numPoints, float desiredPointsPerCell) : m_gridBox(box)
{
        int numCells = (int)ceil(numPoints / desiredPointsPerCell);
	float boxWidth = box.width();
	float boxHeight = box.height();
	float boxRatio = boxWidth/boxHeight;

        m_gridCellsX  =(int) ceil(sqrt(numCells * boxRatio));
        m_gridCellsY = (int)ceil(m_gridCellsX/boxRatio);

    m_gridArray.assign(m_gridCellsX*m_gridCellsY,Cell());
    m_minPointHeight = std::numeric_limits<float>::max();
}

void TriGrid::clear()
{
    m_gridArray.clear();
}

void TriGrid::addTriangle(const Triangle &t)
{
	if(t.p1.y < m_minPointHeight)
		m_minPointHeight = t.p1.y;

	if(t.p2.y < m_minPointHeight)
		m_minPointHeight = t.p2.y;

	if(t.p3.y < m_minPointHeight)
		m_minPointHeight = t.p3.y;

	MinMax2DBox box(t.p1,t.p2,t.p3);
    Pair<int> min = calculateCellForPoint(box.min().x,box.min().y);
    Pair<int> max = calculateCellForPoint(box.max().x,box.max().y);
    if(min[0] < 0) min[0] = 0;
    if(min[1] < 0) min[1] = 0;
    if(max[0] >=m_gridCellsX)
        max[0] = m_gridCellsX-1;
    if(max[1] >=m_gridCellsY)
        max[1] = m_gridCellsY-1;

    for(int y = min[1]; y<=max[1]; ++y)
		for(int x=min[0]; x<=max[0];++x)
		{
			int cell = twoDToArray(x,y);
			m_gridArray[cell].push_back(t);
		}
}

float TriGrid::getHeightAt(float x, float y) const
{
    Ray r(Point3D(x,m_minPointHeight-1.0,y),Vector3D(0,1,0));
	Pair<int> cellpos = calculateCellForPoint(x,y);
	int cell = twoDToArray(cellpos);
	Cell::const_iterator it = m_gridArray[cell].begin();
	Cell::const_iterator end = m_gridArray[cell].end();
	Point3D intersection;
	for(;it!=end;++it)
        if((it->getRayIntersectionPoint(r, &intersection)) == 1)
            return intersection.y;

	return 0;//std::numeric_limits<float>::max();
}

Pair<int> TriGrid::calculateCellForPoint(float x, float y) const
{
        int cellX = (int)((x-m_gridBox.min().x)*m_gridCellsX / m_gridBox.width());
        int cellY = (int)((y-m_gridBox.min().y)*m_gridCellsY / m_gridBox.height());

	if(cellX < 0) cellX = 0;
	if(cellY <0) cellY = 0;
	if(cellX >=m_gridCellsX) cellX = m_gridCellsX-1;
	if(cellY>=m_gridCellsY) cellY = m_gridCellsY-1;

	return Pair<int>(cellX, cellY);
}

